/*
 * FormationSquare.java
 *
 * Created on 18 de noviembre de 2007, 07:04 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package nx.formations;


import nx.utils.Vector2D;
/**
 *
 * @author alejo
 */
public class FormationRow extends Formation {
    
    
    /** Creates a new instance of FormationSquare */
    public FormationRow(int numRobots) {
        super(numRobots);
         
        this.setName("ROW");
        setWidth(numRobots -1); //each robot separated from the other by a meter.
    }
    
    public Vector2D getRobotPosition(int robotId){
        if(robotId>=0 && robotId<=this.getNumRobots()){
            
            //get angle of location in Formation
            float theta = (float)getRobotLocationDir(robotId);
            
            
                
            return getVectorInForm(theta,robotId);
            
        }
            
        return null;
    
    }
    
    
    public Vector2D getVectorInForm(float theta, int robotId){
        
        
        
        float distance = 1;
        float x = 0;
        float y = 0;

        /*
        switch(robotId){
            
            case 0:
                y = 1.0f/3.0f;
            break;
            case 1:
                y = 1;
            break;
            case 2:
                y = -1;
            break;
            case 3:
                y = -1.0f/3.0f;
            break;
            
        }*/
        if(this.getNumRobots() % 2 == 0)
        {
                      
            if(robotId == 0)
                y = distance/2;
            else if(robotId == this.getNumRobots()-1)
                y = -distance/2;
            else if(theta <= Math.PI / 2 + 0.01f) // ok
                y = distance  * (float)(robotId*2+1 - 0.5);
            else if (theta <= Math.PI)
                y =(float)(distance  *(((this.getNumRobots()/2 - 1)-robotId+1)*2 - 0.5));
            else if (theta <= Math.PI + Math.PI / 2)
                y =-(float)(distance  *(((robotId - (this.getNumRobots()/2)+1)*2) - 0.5));
            else
                y = -(float)(distance  *((((this.getNumRobots() - 1)-robotId)*2)+1 - 0.5));
        }
        else
        {
            
            if(robotId == 0)
                y = 0;
            else if(theta <= Math.PI / 2+0.01) // ok
                y = distance  * (float)((robotId)*2);
            else if (theta <= Math.PI)
                y = (float)(distance  *(((this.getNumRobots()-1)/2 -robotId)*2+1));
            else if (theta <= Math.PI + Math.PI / 2)
                y =-(float)(distance  *(((robotId - ((this.getNumRobots()-1)/2+1))*2)+1));
            else
                y = -(float)(distance  *((((this.getNumRobots() - 1)-robotId)*2)+2));
            
        }
        
        x = 0;
        /*
        System.out.print("Angle: ");
        System.out.print(theta);
        System.out.print( "Angle var: ");
        System.out.print(this.getThetaVar());
        System.out.print(" Distance: ");
        
        System.out.println(distance);
        
        
        System.out.print("Robot Id: ");
        System.out.print(robotId);
        System.out.print(" x: ");
        System.out.print(x);
        System.out.print(" y: ");
        System.out.println(y);
        */

        if(robotId>=this.getNumRobots())
            return null;
        else
            return new Vector2D(x,y,Vector2D.MODE_XY);
        
    
    }
    

    public void calculateScaleFactor()
    {
        scaleFactor = (float)(width/2);
    }
    
   
    
}
